#include <iostream>
#include <fstream>
#include <string>
#include "log.h"
#include "init_systems.h"
#include "time_tools.h"
#include "image.h"

#include "modules/Vision/Detector.h"
#include "modules/Vision/StripeDetector.h"
#include "modules/Vision/Normalizer.h"
#include "modules/Vision/Image2DDD.h"
#include "modules/Vision/Image_dir.h"
#include "modules/Util/Conf.h"

using namespace kthrobot;
using namespace Group5;
using namespace std;
int main (int argc, char **argv) {
  Init::RegisterCommon();
  if (!Init::Systems(argc, argv)) return 1;

  Detector*ball_detector;
  Detector*bomb_detector;
  Detector*stripe_detector;
  DetectionResults ball_result;
  DetectionResults bomb_result;
  DetectionResults stripe_result;

  Log::Write("----------------Initializing VisionManager-----------------");
  {
    ifstream ifs(Conf::get().read<std::string>("BallTrainFile").c_str());
    if (!ifs.is_open())
    {
      Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return -1;
    }
    ball_detector = new Detector(ifs);
    Log::Write("Loading Ball Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("BombTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return -1;
    }
    bomb_detector = new Detector(ifs);
    Log::Write("Loading Bomb Detector");
    ifs.close();
  }
  {
    ifstream ifs(Conf::get().read<std::string>("StripeTrainFile").c_str());
    if (!ifs.is_open())
    {
      kthrobot::Log::Write("Unable to load vision training file.");
      kthrobot::Init::SignalShutdown();
      return -1;
    }
    stripe_detector = new StripeDetector(ifs);
    Log::Write("Loading Stripe Detector");
    ifs.close();
  }

  Image2DDD::t11 =  Conf::get().read<int>("T_11");
  Image2DDD::t12 =  Conf::get().read<int>("T_12");
  Image2DDD::t21 =  Conf::get().read<int>("T_21");
  Image2DDD::t22 =  Conf::get().read<int>("T_22");

  Image2DDD::bx =  Conf::get().read<int>("B_X");
  Image2DDD::by =  Conf::get().read<int>("B_Y");

  Image_Iter::get().open_dir("/home/robot/ball_images/");
  Log::Write("inside!");
  for (int i=0;i<1;i++)
  {
    Image* last_image = Image_Iter::get().next_image();
    normalize_image(last_image);
    Log::Write("Normalized");
    ball_detector->detect(*last_image, ball_result,"./res_Ball/Ball_detector");
    {
      int x=0,y=0;
      Image2DDD::Map(ball_result.x,ball_result.y, x, y);
      ball_result.x = x;
      ball_result.y = y;
    }

    if (ball_result.detected)
      stripe_detector->detect(*last_image, stripe_result, "./res_Stripe/Stripe_detector");
    bomb_detector->detect(*last_image, bomb_result, "./res_Bomb/Bomb_detector");
  }

  kthrobot::Init::Shutdown();
  return 0;
}
